基于DSP的欠驅(qū)動體操機器人的搖起控制設計

1 引言  

欠驅(qū)動機器人是一種部分關節(jié)為被動關節(jié)，能用較少的驅(qū)動裝置完成復雜任務的機械系統(tǒng)。欠驅(qū)動機器人是近年來開始出現(xiàn)的一個較新的研究領域，針對欠驅(qū)動機器人系統(tǒng)的研究目前得到不少學者的關注。欠驅(qū)動機器人系統(tǒng)在很多的機器人系統(tǒng)例如多指機器人手、輪式移動機器人、太空機器人等非完整約束系統(tǒng)中都存在。因此近幾年來，欠驅(qū)動機器人的運動控制問題引起國內(nèi)外廣泛的研究興趣。同時，隨著集成芯片技術的飛速發(fā)展，人們對欠驅(qū)動機器人控制的實時性和精度提出了越來越高的要求。  

本文設計了一種基于DSP的機器人控制系統(tǒng)。該控制系統(tǒng)采用兩級控制結構。以通用PC作為上位機，完成目標設定、軌跡生成、系統(tǒng)管理和人機接口等功能；以美國Analog Device公司的定點數(shù)字信號處理芯片ADSP2181為核心，作為機器人控制的下位機。該控制器充分應用了DSP運算的高速性，提高了系統(tǒng)的實時和穩(wěn)定性。  

2 體操機器人硬件系統(tǒng)設計  

2．1 控制系統(tǒng)硬件結構  

控制對象為三關節(jié)欠驅(qū)動體操機器人，其中手臂關節(jié)為被動關節(jié)。控制轉矩來自兩臺直流伺服電機，配5l：l減速器伺服電機，經(jīng)軸線相交的圓錐齒輪傳輸至驅(qū)動關節(jié)，驅(qū)動關節(jié)通過動力耦合作用使被動關節(jié)產(chǎn)生運動。欠驅(qū)動機器人控制系統(tǒng)的任務就是對驅(qū)動關節(jié)電機進行控制，電機運行之前從控制界面輸入控制目標參數(shù)，上位PC機負責發(fā)送控制命令與數(shù)據(jù)采集。DSP處于整個控制系統(tǒng)的底層，主要用來接收來自PC機的控制命令，對驅(qū)動關節(jié)執(zhí)行電機控制，同時把底層信息反饋給上位PC機，以實現(xiàn)機器人運動信息的數(shù)據(jù)分析等功能，用一塊DSP運動控制卡對機器人的肩關節(jié)或髖關節(jié)進行插補計算和伺服控制，采用PCI標準總線進行上、下位機的通訊，實現(xiàn)雙速率運行?？刂葡涿姘逄峁┝烁麝P節(jié)工作狀態(tài)顯示及伺服報警提示，還可以對關節(jié)進行手動控制。三關節(jié)欠驅(qū)動體操機器人控制系統(tǒng)結構如圖l所示，體操機器人本體如圖2所示。  


  
2．2 運動控制器結構  

系統(tǒng)采用ADSP218l數(shù)字信號處理器為核心，實現(xiàn)高性能的控制運算的伺服運動控制器。如圖3所示，運動控制器的控制過程為增量編碼器的A、B相信號作為位置反饋輸入信號，運動控制器通過四倍頻、加減計數(shù)器得到實際位置。實際位置的信息保存在位置寄存器中，PC機可通過控制寄存器讀取。運動控制器的目標位置由PC機設定，通過內(nèi)部計算得到位置誤差，經(jīng)過數(shù)字伺服濾波器后，送到數(shù)模轉換(DAC)或脈寬調(diào)制器(PWM)硬件處理電路，經(jīng)過轉換最后輸出伺服電機的控制信號：+／-lOV模擬信號或PWM信號。  


  
2．3 系統(tǒng)的通訊  

系統(tǒng)采用PCI總線進行通訊。PCI總線的主要優(yōu)點是性能高(數(shù)據(jù)傳輸率可達到132 264Mb／s)，總線通用性強，成本低，使用方便靈活。系統(tǒng)通訊采用PLX9054接口芯片，結合雙口RAM，實現(xiàn)了DSP和PCI總線間的雙向高速實時數(shù)據(jù)交換。PCI總線與雙口RAM的數(shù)據(jù)交換，采用了定時傳送加握手信號的方式進行。具體實現(xiàn)如下：上位機每隔一個固定的時間T下傳一組數(shù)據(jù)，數(shù)據(jù)傳送完成后，發(fā)出一個發(fā)送完信號，下位機接受到這個數(shù)據(jù)后，立即從雙口RAM中讀取數(shù)據(jù)。下位機上傳數(shù)據(jù)也采用同樣的處理方式。這種方式特別適合于機器人控制系統(tǒng)的通訊。  

2．4 驅(qū)動元件的選擇  

驅(qū)動元件選擇了直流伺服電機，其參數(shù)為200W／7220mm／107mN·m Maxon。其驅(qū)動器可實現(xiàn)位置、速度和轉矩三種不同的控制方式：具有共振抑制和控制功能，可彌補機械的剛性不足，從而實現(xiàn)高速定位。同時，還采用PID濾波器，外加速度和加速度前饋，即PID+Kvff+Kaff濾波器。通過調(diào)節(jié)各參數(shù)，濾波器能對大多數(shù)系統(tǒng)實現(xiàn)精確而穩(wěn)定的控制。因此非常適合應用于機器人控制系統(tǒng)。  

3 搖起控制策略  

由于體操機器人大范圍的運動，搖起問題是高度非線性又極具挑戰(zhàn)性的問題。搖起過程如同人在單杠一樣，先使體操機器人來回搖動幾次，體操機器人始終在下，一旦有足夠的能量施加到系統(tǒng)上，機器人即進入倒立狀態(tài)，如圖4所示。  


  
Spong建議一種非線性反饋方法，這種方法利用部分線性反饋定義PD控制器。從直觀上講，當體操機器人從懸掛穩(wěn)定平衡狀態(tài)轉移到倒立不穩(wěn)定平衡狀態(tài)的過程中，其勢能是不斷增加的，因此需要向系統(tǒng)輸入足夠的能量。本研究從能量增加的角度出發(fā)，采用帶有振幅和頻率的正弦方式和斜坡函數(shù)輸入，這樣可同時增加擺動。  


  
實驗證明采用正弦和斜坡函數(shù)，此算法位于基于能量搖起模塊void swing_up()中，這些數(shù)據(jù)分別從運．動控制器的2、1通道輸入，需說明的是，在對體操機器人系統(tǒng)進行數(shù)學建模時已明確，體操機器人系統(tǒng)是一個力控設備，根據(jù)牛頓第二定律，力與加速度成正比，因此給系統(tǒng)施加的控制量就是加速度，而速度則是一個事先設定的較大數(shù)值，在控制過程中，一般不會達到這個速度值。這樣保證產(chǎn)生的附加力隨能量的增加而減小，可搖起體操機器人到倒立平衡位置。  

4 控制系統(tǒng)軟件設計  

實施控制時，主要是控制軟件的編寫與控制其參數(shù)的調(diào)節(jié)。由于體操機器人控制系統(tǒng)的實時性要求較高，控制軟件必須滿足實時性的要求，本系統(tǒng)控制周期為6ms?？刂栖浖仨氃跍蚀_的控制周期內(nèi)完成數(shù)據(jù)采樣，處理并且發(fā)出控制信號給運動控制器。  

首先由上位PC機設置好控制參數(shù)，系統(tǒng)開始運行并完成初始化工作。底層控制器對各轉動關節(jié)進行位置采樣，同時接收來自上位PC機的控制指令，并把兩者結合在一起進行分析，通過編寫進去的控制算法生成相應的轉矩控制信號，經(jīng)功率放大后送給執(zhí)行電機，同時把各關節(jié)的運動信息上傳給PC機，如此反復，完成整個閉環(huán)運動控制。  

下位機控制器上電后主程序進行控制器初始化操作、禁止看門狗、設置關鍵寄存器、設置中斷向量和中斷寄存器、初始化事件管理器、基于能量搖起、進行實時控制模塊等。其中實時控制模塊中用到ADSP2181可編程定時器，它能夠產(chǎn)生周期性的定時中斷，定時間隔是處理器時鐘周期的整數(shù)倍。當定時器被使能后，一個16位的計數(shù)寄存器TCOUNT每隔m個周期就會減1，其初始化程序如下：  

void init_interrupt()  

{disable0；∥關閉中斷(在初始化中斷過程中嚴禁產(chǎn)生中斷)  

outportb(0x43,0x36)；／／寫中斷控制字  

outportb(0x40,0xf6)；∥寫數(shù)據(jù)寄存器：0x40為數(shù)據(jù)寄存器內(nèi)存映射地址  

outportb(0x40,0xlb)；∥數(shù)據(jù)為16位時鐘細分比率，先寫入低字節(jié)0xf6，后寫入高字節(jié)0xlb  

ldhandler=getvect(INTR)；∥保存原有的中斷服務程序句柄  

setvect(INTR，handler)；／／設置新的中斷服務程序句柄  

enable()；∥打開中斷}  

上述中斷初始化代碼創(chuàng)建一個6毫秒的定時器中斷，即當打開中斷后，將每隔6毫秒對體操機器人系統(tǒng)進行一次實時控制。6毫秒的定時時間是通過向數(shù)據(jù)寄存器寫入0x1bf6=7158獲得的。  

5 結語  

基于高性能ADSP2181的控制器設計能夠滿足機器人的搖起控制要求，且由于采用PC+控制器分級控制和模塊化設計思想，將有利于軟、硬件升級，及大大縮短開發(fā)周期。系統(tǒng)在實驗室進行機器人搖起實驗時，各電機軸運轉平穩(wěn)，動作協(xié)調(diào)軌跡跟蹤實時性好，機器人沒有出現(xiàn)抖動、喘振等現(xiàn)象。  

今后還需要探討更多的搖起與平衡控制策略，完善DSP的控制程序。